package team3132.robots.sample;

import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.Talon;
import edu.wpi.first.wpilibj.buttons.JoystickButton;
import edu.wpi.first.wpilibj.command.Command;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import team3132.commands.DriveByJoysticksCommand;
import team3132.commands.NoopCommand;
import team3132.commands.SequentialCommandGroup;
import team3132.commands.StartCompressorCommand;
import team3132.subsystems.CompressorSubsystem;
import team3132.subsystems.DriveSubsystem;
import team3132.utils.NetworkTableValue;
import team3132.utils.OptionalNetworkTableValue;
import team3132.utils.Value;

/**
 * This class is the repository of all the components used by the robot and the glue between
 * Commands and Subsystems.
 */
public class RobotComponents {

  private Value leftJoystickPort;
  private Value rightJoystickPort;
  private Value controllerJoystickPort;
  private Value leftMotorPort;
  private Value rightMotorPort;
  private Value leftEncoderPort1;
  private Value leftEncoderPort2;
  private Value rightEncoderPort1;
  private Value rightEncoderPort2;
  private Value driveTimeout;
  private Value encoderRotationsPerCycle;
  private Value encoderSamplesToAverage;
  private Value compressorSwitchChannel;
  private Value compressorRelayChannel;
  private Value elToroOpenArmsButton;
  private Value elToroCloseArmsButton;
  private Value elToroIntakeButton;
  private Value elToroOutturnButton;
  private Value catapultShootButton;
  private Value catapultRetractButton;
  
  private Joystick leftJoystick;
  private Joystick rightJoystick;
  private Joystick controllerJoystick;
  private SpeedController leftMotor;
  private SpeedController rightMotor;
  private Encoder leftEncoder;
  private Encoder rightEncoder;
  private CompressorSubsystem compressorSubsystem;
  private DriveSubsystem driveSubsystem;
  private Command driveCommand;
  private Command startCompressorCommand;
  private Command autonomousCommand;
  private Command teleopCommand;
  private Command catapultRetractCommand;
  private Command catapultFireCommand;
  private Command elToroOutturnCommand;
  private Command elToroIntakeCommand;
  private Command elToroCloseCommand;
  private Command elToroOpenCommand;

  public RobotComponents() {
  }

  public void init() {
    /*
     * Create the values extracted from the network table. Keep together for visibility.
     * Create the joysticks, the buttons, the low level component using the network table for ports.
     * Create the subsystems passing low level components as required.
     * Create the command passing joysticks, buttons and subsystems as required
     * Create the OI rules to execute commands on joystick/buttons actions
     * 
     * Basically, have all the constructors used here in one file during initialization.
     */

    // Values from the network table
    leftJoystickPort = new NetworkTableValue("left-joystick");
    rightJoystickPort = new NetworkTableValue("right-joystick");
    leftMotorPort = new NetworkTableValue("left-motor-port");
    rightMotorPort = new NetworkTableValue("right-motor-port");
    leftEncoderPort1 = new NetworkTableValue("left-encoder-port1");
    leftEncoderPort2 = new NetworkTableValue("left-encoder-port2");
    rightEncoderPort1 = new NetworkTableValue("right-encoder-port1");
    rightEncoderPort2 = new NetworkTableValue("right-encoder-port2");
    driveTimeout = new OptionalNetworkTableValue("driveTimeout", "100");
    encoderRotationsPerCycle = new OptionalNetworkTableValue("encoder-rotations", "1140");
    encoderSamplesToAverage = new OptionalNetworkTableValue("encoder-samples", "20");
    compressorSwitchChannel = new NetworkTableValue("compressor-switch");
    compressorRelayChannel = new NetworkTableValue("compressor-relay");
    elToroOpenArmsButton = new NetworkTableValue("eltoro-open-button");
    elToroCloseArmsButton = new NetworkTableValue("eltoro-close-button");
    elToroIntakeButton = new NetworkTableValue("eltoro-intake-button");
    elToroOutturnButton = new NetworkTableValue("eltoro-outturn-button");
    catapultShootButton = new NetworkTableValue("catapult-shoot-button");
    catapultRetractButton = new NetworkTableValue("catapult-retract-button");

    // Joysticks
    leftJoystick = new Joystick(leftJoystickPort.getIntValue());
    rightJoystick = new Joystick(rightJoystickPort.getIntValue());
    controllerJoystick = new Joystick(controllerJoystickPort.getIntValue());

    // Motors, sensors and encoders
    // Creating these here make it easy to decide to use the logging wrapper when required.
    leftMotor = new Talon(leftMotorPort.getIntValue());
    rightMotor = new Talon(rightMotorPort.getIntValue());
    leftEncoder = new Encoder(leftEncoderPort1.getIntValue(), leftEncoderPort2.getIntValue());
    rightEncoder = new Encoder(rightEncoderPort1.getIntValue(), rightEncoderPort2.getIntValue());
    leftEncoder.setDistancePerPulse(1.0 / encoderRotationsPerCycle.getIntValue());
    rightEncoder.setDistancePerPulse(1.0 / encoderRotationsPerCycle.getIntValue());
    leftEncoder.setSamplesToAverage(encoderSamplesToAverage.getIntValue());
    rightEncoder.setSamplesToAverage(encoderSamplesToAverage.getIntValue());
    leftEncoder.start();
    rightEncoder.start();

    // Subsystems
    compressorSubsystem = new CompressorSubsystem(compressorSwitchChannel, compressorRelayChannel);
    driveSubsystem = new DriveSubsystem(leftMotor, rightMotor, leftEncoder, rightEncoder);

    // Show what command your subsystem is running on the SmartDashboard
    SmartDashboard.putData(compressorSubsystem);
    SmartDashboard.putData(driveSubsystem);

    // Commands
    catapultRetractCommand = new NoopCommand();
    catapultFireCommand = new NoopCommand();
    elToroOutturnCommand = new NoopCommand();
    elToroIntakeCommand = new NoopCommand();
    elToroCloseCommand = new NoopCommand();
    elToroOpenCommand = new NoopCommand();
    driveCommand = new DriveByJoysticksCommand(driveSubsystem, leftJoystick, rightJoystick, driveTimeout);
    startCompressorCommand = new StartCompressorCommand(compressorSubsystem);
    autonomousCommand = new SequentialCommandGroup(startCompressorCommand);
    teleopCommand = new SequentialCommandGroup(startCompressorCommand, driveCommand);

    // IO actions
    new JoystickButton(controllerJoystick, elToroOpenArmsButton.getIntValue())
        .whenPressed(elToroOpenCommand);

    new JoystickButton(controllerJoystick, elToroCloseArmsButton.getIntValue())
        .whenPressed(elToroCloseCommand);

    new JoystickButton(controllerJoystick, elToroIntakeButton.getIntValue())
        .whileHeld(elToroIntakeCommand);

    new JoystickButton(controllerJoystick, elToroOutturnButton.getIntValue())
        .whileHeld(elToroOutturnCommand);

    new JoystickButton(controllerJoystick,catapultShootButton.getIntValue())
        .whenPressed(catapultFireCommand);

    new JoystickButton(controllerJoystick, catapultRetractButton.getIntValue())
        .whenPressed(catapultRetractCommand);
  }

  public Command getAutonomousCommand() {
    return autonomousCommand;
  }

  public Command getTeleopCommand() {
    return teleopCommand;
  }
}
